

// std
#include <sys/time.h>   // hi-res clock (gettimeofday)
#include <math.h>

// Qt
#include <QDebug>
#include <QLabel>
#include <QDoubleSpinBox>
#include <QGridLayout>

// local
#include "gexception.h"
#include "window.h"



struct timeval Clock;
// -------------------------- StartClock() -----------------------------------
void StartClock()
{
    gettimeofday( &Clock, NULL );
}
// ---------------------------------------------------------------------------


// ------------------------- EndClock() --------------------------------------
QString EndClock( const QString description )
{
    double seconds = 0.0;
    struct timeval end;
    gettimeofday( &end, NULL );

    seconds = (double)end.tv_sec - (double)Clock.tv_sec
			+ 1E-6*end.tv_usec - 1E-6*Clock.tv_usec;
	
    return QString("%1 : %2 [s]").arg( description ).arg( seconds );
}
// ---------------------------------------------------------------------------


#define CLEAR 0xFFFF


// ===========================================================================
//                            GImageTransformation
// ===========================================================================


// ------------ GImageTransformation::GImageTransformation() -----------------
GImageTransformation::GImageTransformation()
{
	_interpolation = Interpolation_NN;
}
// ---------------------------------------------------------------------------

// ---------------------------------------------------------------------------
quint32 & GImageTransformation::operator [] ( const int index )
{
	return _pixels[ index ];
}
// ---------------------------------------------------------------------------


// --------------------- GImageTransformation::clear() -----------------------
void GImageTransformation::clear()
{
	_pixels.fill( CLEAR );
	
// 	_weights.fill( CLEAR );
}
// ---------------------------------------------------------------------------


// ------------------ GImageTransformation::resize() -------------------------
void GImageTransformation::setOutputSize( const QSize & outputSize )
{
	_outputSize = outputSize;
	_pixels.fill( CLEAR, _outputSize.height() * _outputSize.width() );
	_weights.resize( _outputSize.height() * _outputSize.width() );
// 	if( _interpolation == Interpolation_NN )
// 	{
// 		_pixels.fill( CLEAR, _outputSize.height() * _outputSize.width() );
// 	}
// 	else if( _interpolation == Interpolation_Bilinear )
// 	{
// 		_pixels.fill( CLEAR, _outputSize.height() * _outputSize.width() );
// 		_weights.fill( CLEAR, _outputSize.height() * _outputSize.width() );
// 	}
}
// ---------------------------------------------------------------------------


// ------------- GImageTransformation::setInterpolation() --------------------
void GImageTransformation::setInterpolation( const Interpolation interpolation )
{
// 		// Interpolation_NN -> Interpolation_Bilinear
// 	if( ( _interpolation == Interpolation_NN ) && ( interpolation == Interpolation_Bilinear ) )
// 	{
// 	}
// 	
// 		// Interpolation_Bilinear -> Interpolation_NN
// 	if( ( _interpolation == Interpolation_Bilinear ) && ( interpolation == Interpolation_NN ) )
// 	{
// 		_pixels.fill( CLEAR, _outputSize.width() * _outputSize.height() );
// 	}
	
	_pixels.fill( CLEAR, _outputSize.width() * _outputSize.height() );
	_weights.resize( _outputSize.height() * _outputSize.width() );
	_interpolation = interpolation;
	
}
// ---------------------------------------------------------------------------


// -------------------- GImageTransformation::transform() --------------------
void GImageTransformation::transform( int sw, int sh, const char * pSource,
									  int dw, int dh, char * pDest, Format format )
{
	Q_ASSERT( sw == _inputSize.width() );
	Q_ASSERT( sh == _inputSize.height() );
	Q_ASSERT( dw == _outputSize.width() );
	Q_ASSERT( dh == _outputSize.height() );

	
	switch( format )
	{
		case Format_Indexed8:
			transformFormat_Indexed8( sw, sh, pSource, dw, dh, pDest );
			break;
		
		case Format_RGB24:
			transformFormat_RGB24( sw, sh, pSource, dw, dh, pDest );
			break;	
		
		default:
			break;
	}
}
// ---------------------------------------------------------------------------


// ------------ GImageTransformation::transformFormat_Indexed8() -------------
void GImageTransformation::transformFormat_Indexed8( int sw, int sh,
		const char * pSource, int dw, int dh, char * pDest )
{
	StartClock();
	switch( _interpolation )
	{
		case Interpolation_NN:
		{
			int size = dw*dh;
			quint32 * pPixels = _pixels.data();
			
			for( int i = 0; i < size; i++)
			{
				*( pDest++) = *(pSource + *(pPixels++) );
			}
		}
		break;
		
		case Interpolation_Bilinear:
		{
			int size = dw*dh;
			Weight * pWeights = _weights.data();
			int counter = 0;
			for( int i = 0; i < size; i++)
			{
				quint32 value = ( *(pSource + pWeights->p1 ) * pWeights->w1
						+ *(pSource + pWeights->p2 ) * pWeights->w2
						+ *(pSource + pWeights->p3 ) * pWeights->w3
						+ *(pSource + pWeights->p4 ) * pWeights->w4 ) / 1024 / 4;
				if( value > 255 ) counter++;

				
// 				quint32 value = ( *(pSource + pWeights->p1 ) * pWeights->w1
// 						+ *(pSource + pWeights->p2 ) * pWeights->w2
// 						+ *(pSource + pWeights->p3 ) * pWeights->w3
// 						+ *(pSource + pWeights->p4 ) * pWeights->w4 ) >> 17;
				*( pDest++ ) = (quint8)value;
				pWeights++;
			}
			qDebug() << "counter" << counter << size;
			
		}
		break;
		
		default:
			break;
	}
		
	qDebug() << EndClock( "Transforming" );
}
// ---------------------------------------------------------------------------


// ------------- GImageTransformation::transformFormat_RGB24() ---------------
void GImageTransformation::transformFormat_RGB24( int sw, int sh,
		const char * pSource, int dw, int dh, char * pDest )
{
	// TODO Format_RGB24 translating 
}
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
QImage GImageTransformation::transform( const QImage & source )
{
	int sw = source.width();
	int sh = source.height();
	int dw = sw;
	int dh = sh;
	QByteArray sourceImageData( sw * sh, 0 );
	
	for( int y = 0; y < sh; y++ )
	{
		for( int x = 0; x < sw; x++ )
		{
			sourceImageData[ y * sw + x ] = qGray( source.pixel( x, y ) );
		}
	}
	
	QByteArray destImageData( dw * dh, 0 );
	
	transform( sw, sh, sourceImageData.data(),
			   dw, dh, destImageData.data() );
	
	
	QImage destImage( dw, dh, QImage::Format_RGB32 );		
	char * pBbyte = destImageData.data();
	for( int y = 0; y < dh; y++ )
	{
		for( int x = 0; x < dw; x++ )
		{
			if ( pBbyte < destImageData.data() + destImageData.size()  )
			{
				destImage.setPixel( x, y, qRgb( *pBbyte, *pBbyte, *pBbyte ) );
				pBbyte++;
			}
			else
			{
				qWarning("image buffer smaller than expected, buf size:"
						" %d, image: %d pixels", destImageData.size(), dw*dh );
			}
		}
	}
	
	return destImage;
}
// ---------------------------------------------------------------------------




// ===========================================================================
//                                  Window
// ===========================================================================


// ---------------------------- Window::Window() -----------------------------
Window::Window( QWidget * pParent, Qt::WindowFlags f )
	: QWidget( pParent, f )
{
		// ui
	setupUi( this );
	
	connect( &_udpReceiver, SIGNAL(packetReceived( int , const QByteArray&  )),
		SLOT( udpPacketReceived( int , const QByteArray&  ) ) );
	
	connect( &_udpReceiver, SIGNAL(packetsReceived( const QQueue<QPair<quint16,QByteArray>>& )),
		SLOT( udpPacketsReceived( const QQueue<QPair<quint16,QByteArray>>& ) ) );

	
// 	_sourceImage.load( "hoods_small.jpg" );
// 	_sourceImage.load( "small.jpg" );
	_sourceImage.load( "gray.jpg" );
// 	_sourceImage.load( "g5094.png" );
// 	_sourceImage.load( "g4058.png" );
	_resultImage = QImage( _sourceImage );
	if( labelImage )
	{
		labelImage->setPixmap( QPixmap::fromImage( _resultImage ) );
	}
	
	connect( spinRotX, SIGNAL( valueChanged( int ) ), this, SLOT( createTransformation() ) );
	connect( spinRotY, SIGNAL( valueChanged( int ) ), this, SLOT( createTransformation() ) );
	connect( spinRotZ, SIGNAL( valueChanged( int ) ), this, SLOT( createTransformation() ) );
	connect( doubleSpinScaleX, SIGNAL( valueChanged( double ) ), this, SLOT( createTransformation() ) );
	connect( doubleSpinScaleY, SIGNAL( valueChanged( double ) ), this, SLOT( createTransformation() ) );
	
}
// ---------------------------------------------------------------------------


// ------------------- Window::createTransformation() ------------------------
void Window::createTransformation()
{
	StartClock();
	_transformation.clear();
	_transformation.setInputSize( _sourceImage.size() );
// 	_transformation.setInterpolation( GImageTransformation::Interpolation_Bilinear );
	_transformation.setInterpolation( GImageTransformation::Interpolation_NN );
	_transformation.setOutputSize( _sourceImage.size() );
	
	double x0 = _sourceImage.width() / 2;
	double y0 = _sourceImage.height() / 2;
	
	QTransform matrix;
	matrix.rotate( spinRotX->value(), Qt::XAxis );
	matrix.rotate( spinRotY->value(), Qt::YAxis );
	matrix.rotate( spinRotZ->value(), Qt::ZAxis );
	matrix.scale( doubleSpinScaleX->value(), doubleSpinScaleY->value() );
	matrix.translate( -x0, -y0 );
	
	Log( toString( matrix ) );
	
	QPolygon originalPoly;
	originalPoly.append( QPoint( 0, 0 ) );
	originalPoly.append( QPoint( 0, _sourceImage.width() ) );
	originalPoly.append( QPoint( _sourceImage.height(), _sourceImage.width() ) );
	originalPoly.append( QPoint( _sourceImage.height(), 0 ) );	
			
	QPolygon poly = matrix.map( originalPoly );
	QTransform trans;
	if (!QTransform::quadToQuad( originalPoly, poly, trans))
		qWarning("QTransform::quadToQuad bad...");
	
	
	for( int y = 0; y < _sourceImage.height(); y++ )
	{
		for( int x = 0; x < _sourceImage.width(); x++ )
		{
			QPointF mappedPoint = trans.map( QPointF( x, y ) );
			double xs = mappedPoint.x() + x0;
			double xf = floor( xs );
			double xc = ceil( xs );
			double ys = mappedPoint.y() + y0;
			double yf = floor( ys );
			double yc = floor( ys );
			
			if( xs < 1 || xs >= (_sourceImage.width()-1) )
			{
				continue;
			}
			if( ys < 1 || ys >= (_sourceImage.height()-1) )
			{
				continue;
			}
			
			Weight weight;

			double max = sqrt( 2 );
			weight.p1 = (quint32)( yf*_sourceImage.width() + xf );
			double value1 = sqrt( (xs-xf)*(xs-xf) + (ys-yf)*(ys-yf) );
			weight.w1 = (quint16)( value1/max*1024);
			
			weight.p2 = (quint32)( yf*_sourceImage.width() + xc );
			double value2 = sqrt( (xc-xs)*(xc-xs) + (ys-yf)*(ys-yf) );
			weight.w2 = (quint16)( value2/max*1024 );
			
			weight.p3 = (quint32)( yf*_sourceImage.width() + xf );
			double value3 = sqrt( (xc-xs)*(xc-xs) + (yc-ys)*(yc-ys) );
			weight.w3 = (quint16)( value3/max*1024 );
			
			weight.p4 = (quint32)( yf*_sourceImage.width() + xf );
			double value4 = sqrt( (xs-xf)*(xs-xf) + (yc-ys)*(yc-ys) );
			weight.w4 = (quint16)( value4/max*1024 );
			
			_transformation.setWeight( y*_sourceImage.width() + x, weight );

				// find nearest neighbour 
			QVector<double> distances;
			distances << value1 << value2 << value3 << value4;
			qSort( distances );
			double nd = distances.first();
			
			if( nd == value1 )
				_transformation[ y*_sourceImage.width() + x ] = weight.p1;
			else if( nd == value2 )
				_transformation[ y*_sourceImage.width() + x ] = weight.p2;
			else if( nd == value3 )
				_transformation[ y*_sourceImage.width() + x ] = weight.p3;
			else 
				_transformation[ y*_sourceImage.width() + x ] = weight.p4;
				
// 			_transformation[ y*_sourceImage.width() + x ] = ys*_sourceImage.width() + xs;
			
		}
	}
	qDebug() << EndClock("creating transformation");
	transform();
}
// ---------------------------------------------------------------------------


// --------------------------- Window::transform() ---------------------------
void Window::transform()
{
	if( labelImage )
	{
		QImage image = _transformation.transform( _sourceImage );
		labelImage->setPixmap( QPixmap::fromImage( image ) );
	}
}
// ---------------------------------------------------------------------------


// ---------------------- Window::toString() ---------------------------------
QString Window::toString( QTransform & transformation )
{
	QString line( "\n" );
	line.append( QString( "%1 %2 %3\n" )
			.arg( transformation.m11(), 15, 'f', 15 )
			.arg( transformation.m12(), 15, 'f', 15 )
			.arg( transformation.m13(), 15, 'f', 15 ) );
	line.append( QString( "%1 %2 %3\n" )
			.arg( transformation.m21(), 15, 'f', 15 )
			.arg( transformation.m22(), 15, 'f', 15 )
			.arg( transformation.m23(), 15, 'f', 15 ) );
	line.append( QString( "%1 %2 %3\n" )
			.arg( transformation.m31(), 15, 'f', 15 )
			.arg( transformation.m32(), 15, 'f', 15 )
			.arg( transformation.m33(), 15, 'f', 15 ) );
	
	return line;
}
// ---------------------------------------------------------------------------


// ----------------------- Window::on_pushReceive_toggled() ------------------
void Window::on_pushReceive_toggled( bool b )
{
	if( b )
	{
		if ( _udpReceiver.isReceiving() )
		{
			_udpReceiver.stopReceiving();
		}
		Q_ASSERT( spinPort );
		try
		{
			_udpReceiver.startReceiving( spinPort->value() );
		}
		catch( const GException& e )
		{
			Log( QString("Exception on startReceiving:") + e.getMessage() );
		}
	}
	else
	{
			_udpReceiver.stopReceiving();
	}
}
// ---------------------------------------------------------------------------


// --------------------- Window::udpPacketReceived() -------------------------
/// Processes single UDP packet
void Window::udpPacketReceived( int packetId, const QByteArray& data )
{
	_data = data;
	//Log( QString("Packet %1 arrived, size: %2").arg( packetId ). arg( data.size() ) );
	dataReaded();
}
// ---------------------------------------------------------------------------

// ---------------- Window::udpPacketsReceived() -----------------------------
/// Processes multiple UDP packets, i congestion-control mode
void Window::udpPacketsReceived( const QQueue< QPair< quint16, QByteArray> >& packets )
{
	//Log( QString("%1 packets received").arg( packets.size() ) );
	qDebug("%d packets recieved", packets.size() );
	
	// display only last (more recent) frame
	_data = packets.last().second;
	if ( packets.size() > 1 )
	{
		qDebug("data size: %d", _data.size() );
	}
	dataReaded();
}
// ---------------------------------------------------------------------------


// --------------------  Window::on_buttonSave_clicked() ---------------------
/// Saves current image to file
void Window::on_buttonSave_clicked()
{
	QPixmap pixmap = *labelImage->pixmap();
	QString path = QFileDialog::getSaveFileName( this, tr("Select target image file"),
		QDir::currentPath(), "Images (*.png *.xpm *.jpg)" );
	
	if ( ! path.isEmpty() )
	{
		pixmap.save( path );
	}
}
// ---------------------------------------------------------------------------


// -------------------- Window::on_pushOpen_clicked() ------------------------
void Window::on_pushOpen_clicked()
{
	QString path = QFileDialog::getOpenFileName( this, tr("Select target image file"),
			QDir::currentPath(), "Images (*.png *.xpm *.jpg)" );
	
	if( !path.isEmpty() )
	{
		Q_ASSERT( labelImage );
		QPixmap pixmap( path );
		labelImage->setPixmap( pixmap );
		_sourceImage = pixmap.toImage();
	}
}
// ---------------------------------------------------------------------------


// ----------------------  Window::dataReaded() ------------------------------
void Window::dataReaded()
{
	// disabled to improve performance
	//Log( QString("Frame get: %1 bytes").arg( _data.size() ) );
	
	// read and decode image
	QDataStream stream( _data );
	
	int width;
	int height;
	int format; // 0 - raw grayscale, 1- jpeg2k
	QByteArray sourceImageData;
	QPixmap pixmap;
	
	stream >> width;
	stream >> height;
	stream >> format;
	stream >> sourceImageData;

	qDebug("received width: %d, height: %d", width, height );
	
	// decode raw
	if ( format == 0 )
	{
		QByteArray destImageData( sourceImageData.size(), 0 );

		_transformation.transform( width, height, sourceImageData.data(),
								   width, height, destImageData.data() );
		
		QImage image( width, height, QImage::Format_RGB32 );
		
		char* byte = destImageData.data();
		for( int y = 0; y < height; y++ )
		{
			for( int x = 0; x< width; x++ )
			{
				if ( byte< destImageData.data() + destImageData.size()  )
				{
					image.setPixel( x, y, qRgb( *byte, *byte, *byte ) );
					byte++;
				}
				else
				{
					qWarning("image buffer smalles than expected, buf size:"
							" %d, image: %d pixels", destImageData.size(), width*height );
				}
			}
		}
		
		pixmap = QPixmap::fromImage( image );
	}
	// decode jpeg2k
	else if ( format == 1 )
	{
		pixmap.loadFromData( sourceImageData, "j2c" );
		QFile f( "image.j2c" );
		if ( f.open( QIODevice::WriteOnly | QIODevice::Truncate ) )
		{
			f.write( sourceImageData );
		}
	}
	// unknown
	else
	{
		qWarning("Unknown format: %d", format );
	}
	
	// display decoded pixmap
	if ( ! pixmap.isNull() )
	{
		labelImage->setPixmap( pixmap );
		labelImage->repaint();
	}
}
// ---------------------------------------------------------------------------


// ------------------------------ Window::Log() ------------------------------
void Window::Log( const QString& text )
{
	textLog->append( text );
}
// ---------------------------------------------------------------------------





// eof 
